
#include "fc_error.h"
#include "zino.h"
#include "maths.h"
#include <rtthread.h>
#define DBG_TAG "fe_error"
#define DBG_LVL DBG_ERROR
#include <rtdbg.h>
#include "usercmd.h"

/**
 * 			快闪					慢闪			长亮
1.	加速度计硬件错误				水平校准完成					    红色
2.	气压计硬件错误												       紫色
3.	无线硬件错误					对频中						       蓝色
5.								   信号丢失						     黄色
6.  光流硬件错误													 黄色蓝色
7.	罗盘硬件错误					罗盘校准中						  红色绿色
8. 									待机							彩虹
9.													解锁（飞行）	 绿色
 */

#define FC_ERROR_COLOR(X) X##_ERROR_COLOR

const uint8_t ACC_ERROR_COLOR[3] = {125, 0, 0};                                                                                      // 红色
const uint8_t BARO_ERROR_COLOR[3] = {102, 0, 204};                                                                                   // 紫色
const uint8_t NRF_ERROR_COLOR[3] = {0, 0, 255};                                                                                      // 蓝色
const uint8_t RC_ERROR_COLOR[3] = {255, 255, 0};                                                                                     // 黄色                                                                                                                                //黄色                                                                                                                               //黄色
const uint8_t OPTICALFLOW_ERROR_COLOR[6] = {255, 255, 0, 0, 0, 255};                                                                 // 黄色蓝色
const uint8_t COMPASS_ERROR_COLOR[6] = {255, 0, 0, 0, 255, 255};                                                                     // 红色青色
const uint8_t STANDBY_ERROR_COLOR[21] = {255, 0, 0, 255, 102, 0, 255, 255, 51, 51, 255, 51, 51, 51, 255, 102, 0, 204, 153, 51, 153}; // 彩虹
const uint8_t INDOORSTANDBY[3] = {0, 0, 255};                                                                                        // 蓝色呼吸
const uint8_t OUTDOORSTANDBY[3] = {255, 255, 255};                                                                                   // 白色
const uint8_t ARMED_ERROR_COLOR[3] = {0, 255, 0};                                                                                    // 绿色
const uint8_t LOW_POWER_ERROR_COLOR[3] = {255, 80, 0};                                                                               // 橘黄色

// uint32_t fc_error_flag = 0;
bool rgb_led_default_set = true;

const fc_error_s fc_errors[FC_ERROR_COUNT] =
    {
        [FC_ACC_GYRO_HARDFAULT] =
            {
                .color = ACC_ERROR_COLOR,
                .colorCount = 1,
                .blinkType = FC_ERROR_BLINK_FAST,
            },
        [FC_BARO_HARDFAULT] =
            {

                .color = BARO_ERROR_COLOR,
                .colorCount = 1,
                .blinkType = FC_ERROR_BLINK_FAST,
            },
        [FC_LASER_HARDFAULT] =
            {

                .color = BARO_ERROR_COLOR,
                .colorCount = 1,
                .blinkType = FC_ERROR_BLINK_FAST,
            },
        [FC_RC_HARDFAULT] =
            {

                .color = NRF_ERROR_COLOR,
                .colorCount = 1,
                .blinkType = FC_ERROR_BLINK_FAST,
            },
        [FC_OPTICALFLOW_HARDFAULT] =
            {

                .color = OPTICALFLOW_ERROR_COLOR,
                .colorCount = 2,
                .blinkType = FC_ERROR_BLINK_FAST,
            },
        // [FC_COMPASS_HARDFAULT] =
        //     {

        //         .color = COMPASS_ERROR_COLOR,
        //         .colorCount = 2,
        //         .blinkType = FC_ERROR_BLINK_FAST,
        //     },
        [FC_IMU_CALIBRATING] =
            {

                .color = ACC_ERROR_COLOR,
                .colorCount = 1,
                .blinkType = FC_ERROR_BLINK_SLOW,
            },
        [FC_RC_PAIRING] =
            {

                .color = NRF_ERROR_COLOR,
                .colorCount = 1,
                .blinkType = FC_ERROR_BLINK_SLOW,
            },
        [FC_RC_LOST] =
            {

                .color = RC_ERROR_COLOR,
                .colorCount = 1,
                .blinkType = FC_ERROR_BLINK_SLOW,
            },
        // [FC_COMPASS_CALIBRATING] =
        //     {

        //         .color = COMPASS_ERROR_COLOR,
        //         .colorCount = 2,
        //         .blinkType = FC_ERROR_BLINK_SLOW,
        //     },
        [FC_STANDBY] =
            {

                .color = STANDBY_ERROR_COLOR,
                .colorCount = 7,
                .blinkType = FC_ERROR_BLINK_SLOW,
            },
        [FC_OK_TO_ARM] =
            {

                .color = ARMED_ERROR_COLOR,
                .colorCount = 1,
                .blinkType = FC_ERROR_BLINK_SLOW,
            },

        [FC_EMERGENCY_STOP] =
            {

                .color = ARMED_ERROR_COLOR,
                .colorCount = 1,
                .blinkType = FC_ERROR_BLINK_FAST,
            },
        [FC_ARMING_ALERK] =
            {

                .color = ARMED_ERROR_COLOR,
                .colorCount = 1,
                .blinkType = FC_ERROR_BLINK_SLOW,
            },
        [FC_LOW_POWER] =
            {
                .color = LOW_POWER_ERROR_COLOR,
                .colorCount = 1,
                .blinkType = FC_ERROR_BLINK_SLOW,
            },
        [FC_ARMED] =
        {

            .color = ARMED_ERROR_COLOR,
            .colorCount = 1,
            .blinkType = FC_ERROR_BLINK_BREATH,
        },
        // [FC_MODE_STANDARD_OUTDOOR] =
        //     {
        //         .color = 0,
        //         .colorCount = 0,
        //         .blinkType = 0,

        //     },

};

void new_rain_bow_breath()
{
    // uint8_t rSet = 0;
    // uint8_t gSet = 0;
    // uint8_t bSet = 0;
    static bool rMax = false;
    static bool gMax = false;
    static bool bMax = false;

    static uint8_t r = 0, g = 0, b = 0;

    if (r < 254 && !rMax)
    {
        r += 2;
    }
    else if (r == 254)
    {
        rMax = true;
    }
    if (rMax && r > 0)
    {
        r -= 2;
    }
    if (rMax && (g < 254) & !gMax)
    {
        g += 2;
    }
    else if (g == 254)
    {
        gMax = true;
    }
    if (gMax && g > 0)
    {
        g -= 2;
    }
    if (gMax && b < 254 && !bMax)
    {
        b += 2;
    }
    else if (b == 254)
    {
        bMax = true;
    }
    if (bMax && b > 0)
    {
        b -= 2;
    }
    if (bMax && b == 0)
    {
        rMax = false;
        gMax = false;
        bMax = false;
        r = 0;
        g = 0;
        b = 0;
    }
    // usartPrintf("r = %d,g = %d, b = %d\n", r, g, b);
    // usartPrintf("rMax = %d,gMax = %d, bMax = %d\n", rMax, gMax, bMax);
    rgb_set_color_sum(5, constrain(lrintf(r), 0, 255), constrain(lrintf(g), 0, 255), constrain(lrintf(b), 0, 255));
}
void rain_bow_breath(uint8_t *COLOR, uint8_t len, float BreathTime)
{
    static uint8_t *_COLOR = 0;
    const uint8_t *RainBow = COLOR;
    // ���ý����ͽ���һ��Ҫ�õĲ���������Խ�࣬������ʱ���Խ���������ÿ10����ִ��һ�ξ���10*200=2000����
    //  const float BreathTime = 100.0f;
    // ���ܱ������ڴ�ı�����¼��ɫ���У���ɫ����Ĺ���Ϊ��r,g,b,r,g,b,r,g,b...
    static uint8_t cnt = 1;
    // ��ȡ��ɫ���к�ɫֵ
    uint8_t rSet = RainBow[(int)(cnt - 1)];
    // ��ȡ��ɫ������ɫֵ
    uint8_t gSet = RainBow[(int)((cnt + 1) - 1)];
    // ��ȡ��ɫ������ɫֵ
    uint8_t bSet = RainBow[(int)((cnt + 2) - 1)];
    // Ҫ��ʾ�ĺ�ɫֵ
    static float r = 0;
    // Ҫ��ʾ����ɫֵ
    static float g = 0;
    // Ҫ��ʾ����ɫֵ
    static float b = 0;
    // �����ɫ����ʱ�ı仯�����仯��ΪҪ��ʾ�����ȳ��Ժ����Ĳ����ٳ���2��һ��һ���������ǳ���2��
    float rStep = (rSet / (BreathTime / 2.0f));
    // ������ɫ����ʱ�ı仯��
    float gStep = (gSet / (BreathTime / 2.0f));
    // ������ɫ����ʱ�ı仯��
    float bStep = (bSet / (BreathTime / 2.0f));
    // �������л���־λ��һ��һ������Ӧ��������
    static bool breath = 1;
    if (_COLOR != COLOR)
    {
        _COLOR = COLOR;
        cnt = 1;
        r = 0;
        g = 0;
        b = 0;
    }
    if (breath)
    {
        // ����ʱ��ÿ��Ҫ��ʾ����ɫ�������ӵ��趨����
        if (r <= rSet)
        {
            r = r + rStep;
        }
        if (g <= gSet)
        {
            g = g + gStep;
        }
        if (b <= bSet)
        {
            b = b + bStep;
        }
        if (r >= rSet)
        {
            if (g >= gSet)
            {
                if (b >= bSet)
                {
                    // ÿ��Ҫ��ʾ����ɫ�����ӵ��趨����ʱ��������־λ��ת
                    breath = !breath;
                }
            }
        }
    }
    else
    {
        // ����ʱ��ÿ��Ҫ��ʾ����ɫ������С����
        if (r >= 0)
        {
            r = r - rStep;
        }
        if (g >= 0)
        {
            g = g - gStep;
        }
        if (b >= 0)
        {
            b = b - bStep;
        }
        if (r <= 0 && (g <= 0 && b <= 0))
        {
            // ÿ��Ҫ��ʾ����ɫ����С����ʱ��������־λ��ת
            // ͬʱ��ɫ���м�����3���죬�����̹���3����ɫ�����Լ�3��
            breath = !breath;
            cnt = cnt + 3;
            r = 0;
            g = 0;
            b = 0;
        }
    }
    if (cnt >= len)
    {
        // ��ɫ���м����ӵ����߳����趨�������ɫ����ʱ���ص���һ����ɫ
        cnt = 1;
    }
    // ��ʾ�����еĺ�ɫ����ɫ����ɫ
    rgb_set_color_sum(5, constrain(lrintf(r), 0, 255), constrain(lrintf(g), 0, 255), constrain(lrintf(b), 0, 255));
    // usartPrintf("rgb:%d,%d,%d\n", lrintf(r), lrintf(g), lrintf(b));
}
void fc_error_info_mesg(void)
{

    if (FC_ERROR_CHECK(FC_ACC_GYRO_HARDFAULT))
        LOG_E("\n*%d* FC_ACC_GYRO_HARDFAULT\n", FC_ACC_GYRO_HARDFAULT);
    if (FC_ERROR_CHECK(FC_BARO_HARDFAULT))
        LOG_E("\n*%d* FC_BARO_HARDFAULT\n", FC_BARO_HARDFAULT);
    if (FC_ERROR_CHECK(FC_RC_HARDFAULT))
        LOG_E("\n*%d* FC_RC_HARDFAULT\n", FC_RC_HARDFAULT);
    if (FC_ERROR_CHECK(FC_OPTICALFLOW_HARDFAULT))
        LOG_E("\n*%d* FC_OPTICALFLOW_HARDFAULT\n", FC_OPTICALFLOW_HARDFAULT);
    if (FC_ERROR_CHECK(FC_COMPASS_HARDFAULT))
        LOG_E("\n*%d* FC_COMPASS_HARDFAULT\n", FC_COMPASS_HARDFAULT);
    if (FC_ERROR_CHECK(FC_IMU_CALIBRATING))
        LOG_E("\n*%d* FC_IMU_CALIBRATING\n", FC_IMU_CALIBRATING);
    if (FC_ERROR_CHECK(FC_RC_PAIRING))
        LOG_E("\n*%d* FC_RC_PAIRING\n", FC_RC_PAIRING);
    if (FC_ERROR_CHECK(FC_RC_LOST))
        LOG_E("\n*%d* FC_RC_LOST\n", FC_RC_LOST);
    if (FC_ERROR_CHECK(FC_IMU_CALIBRATING))
        LOG_E("\n*%d* FC_IMU_CALIBRATING\n", FC_IMU_CALIBRATING);
    if (FC_ERROR_CHECK(FC_COMPASS_CALIBRATING))
        LOG_E("\n*%d* FC_COMPASS_CALIBRATING\n", FC_COMPASS_CALIBRATING);
    if (FC_ERROR_CHECK(FC_LASER_HARDFAULT))
        LOG_E("\n*%d* FC_LASER_HARDFAULT\n", FC_LASER_HARDFAULT);
    // if (isFC_NO_ERROR())
    // {
    //     if (fc_mode_record == 0)
    //         usartPrintf("\n*%d* FC_STANDBY\n", fc_mode_record);
    //     else if (fc_mode_record == 1)
    //         usartPrintf("\n*%d* FC_MODE_STANDARD_INDOOR\n", fc_mode_record);
    //     else if (fc_mode_record == 2)
    //         usartPrintf("\n*%d* FC_MODE_STANDARD_OUTDOOR\n", fc_mode_record);

    if (FC_ERROR_CHECK(FC_OK_TO_ARM))
        LOG_E("\n*%d* FC_OK_TO_ARM\n", FC_OK_TO_ARM);
    if (FC_ERROR_CHECK(FC_EMERGENCY_STOP))
        LOG_E("\n*%d* FC_EMERGENCY_STOP\n", FC_EMERGENCY_STOP);
    if (FC_ERROR_CHECK(FC_ARMED))
        LOG_E("\n*%d* FC_ARMED\n", FC_ARMED);
    if (FC_ERROR_CHECK(FC_ARMING_ALERK))
        LOG_E("\n*%d* FC_ARMING_ALERK\n", FC_ARMING_ALERK);
    //     if (FC_ERROR_CHECK(FC_LOW_POWER))
    //         LOG_E("\n*%d* FC_LOW_POWER\n", FC_LOW_POWER);
    //     usartPrintf("\nRC_LOST_CHECKED:%d\n", RC_LOST_CHECKED);
    // }
}

void fc_error_rgb_handle()
{

    if (!rgb_led_default_set)
        return;

    // FC_ERROR_RESET(FC_ACC_GYRO_HARDFAULT);
    // FC_ERROR_RESET(FC_BARO_HARDFAULT);
    // FC_ERROR_RESET(FC_RC_HARDFAULT);
    // FC_ERROR_RESET(FC_OPTICALFLOW_HARDFAULT);
    // FC_ERROR_RESET(FC_COMPASS_HARDFAULT);
    // FC_ERROR_RESET(FC_RC_PAIRING);
    // FC_ERROR_RESET(FC_ACC_CALIBRATING);
    // FC_ERROR_RESET(FC_RC_LOST);
    // FC_ERROR_RESET(FC_COMPASS_CALIBRATING);
    // FC_ERROR_RESET(FC_STANDBY);
    // FC_ERROR_RESET(FC_ARMED);

    // FC_ERROR_SET(FC_ACC_GYRO_HARDFAULT);
    // FC_ERROR_SET(FC_BARO_HARDFAULT);
    // FC_ERROR_SET(FC_RC_HARDFAULT);
    // FC_ERROR_SET(FC_OPTICALFLOW_HARDFAULT);
    // FC_ERROR_SET(FC_COMPASS_HARDFAULT);
    // FC_ERROR_SET(FC_RC_PAIRING);
    //  FC_ERROR_SET(FC_IMU_CALIBRATING);
    // FC_ERROR_SET(FC_RC_LOST);
    // FC_ERROR_SET(FC_COMPASS_CALIBRATING);
    //   FC_ERROR_SET(FC_STANDBY);
    // FC_ERROR_SET(FC_ARMED);

    if (isFC_NO_ERROR())
    {

        new_rain_bow_breath();
        rgb_show();
        return;
    }

    uint32_t currentMs = millis();

    static uint8_t errorCheck;
    static uint32_t errorCheckStartT;
    static uint32_t preDt = 0;
    static bool flag;
    // static uint8_t colorSwithc;
    // static uint8_t colorShowed;
    static uint8_t blinkTimes;

    // FC_ERROR_CYCLE_TIME for every status
    if (currentMs > (errorCheckStartT + FC_ERROR_CYCLE_TIME))
    {
        errorCheck++;
        flag = 0;
        // colorSwithc = 0;
        preDt = 0;
        blinkTimes = 0;
        errorCheckStartT = currentMs;
    }
    //   usartPrintf("currentMs:%d\n",currentMs);
    // find a error
    while (!FC_ERROR_CHECK(errorCheck))
    {
        errorCheck++;
        if (errorCheck == FC_ERROR_COUNT)
        {
            preDt = 0;
            errorCheck = 0;
            break;
        }
    }

    uint32_t Dt = currentMs - errorCheckStartT; // from errorCheckStartT to now

    if (fc_errors[errorCheck].blinkType == FC_ERROR_BLINK_NO)
    {
        rgb_set_color_sum(5, fc_errors[errorCheck].color[0], fc_errors[errorCheck].color[1], fc_errors[errorCheck].color[2]);
    }
    else if (fc_errors[errorCheck].blinkType == FC_ERROR_BLINK_SLOW)
    {
        // usartPrintf(" dt:%d p:%d\n",Dt,preDt);
        if (Dt > (preDt + FC_ERROR_BLINK_SLOW_TIME))
        {
            // usartPrintf("FC_ERROR_BLINK_SLOW_TIME\n");
            preDt = Dt;
            // usartPrintf("colorCount:%d\n",fc_errors[errorCheck].colorCount);
            if (fc_errors[errorCheck].colorCount == 1)
            {

                if (!flag)
                {

                    rgb_set_color_sum(5, fc_errors[errorCheck].color[0], fc_errors[errorCheck].color[1], fc_errors[errorCheck].color[2]);
                    // usartPrintf("%d,%d,%d\n",fc_errors[errorCheck].color[0], fc_errors[errorCheck].color[1], fc_errors[errorCheck].color[2]);
                }
                else
                {

                    rgb_set_color_sum(5, 0, 0, 0);

                    // blinkTimes++;
                }
                flag = !flag;
                // usartPrintf("%d, %d,%d,%d\n",flag, fc_errors[errorCheck].color[0], fc_errors[errorCheck].color[1], fc_errors[errorCheck].color[2]);
            }
            else if (fc_errors[errorCheck].colorCount == 2)
            {
                static uint8_t fcnt;
                if (fcnt == 0)
                {

                    rgb_set_color_sum(5, fc_errors[errorCheck].color[0], fc_errors[errorCheck].color[1], fc_errors[errorCheck].color[2]);
                }
                else if (fcnt == 1)
                {

                    rgb_set_color_sum(5, fc_errors[errorCheck].color[3], fc_errors[errorCheck].color[4], fc_errors[errorCheck].color[5]);
                }
                // rgb_set_color(0,0, 0, 0);
                // }

                fcnt++;

                if (fcnt > 2)
                {
                    fcnt = 0;

                    rgb_set_color_sum(5, 0, 0, 0);
                    // blinkTimes++;
                }

                // if (blinkTimes == FC_ERROR_BLINK_FAST_COUNT)
                // {
                //     rgb_set_color(0,0, 0, 0);
                // }
            }
        }
    }
    else if (fc_errors[errorCheck].blinkType == FC_ERROR_BLINK_FAST)
    {

        if (blinkTimes == FC_ERROR_BLINK_FAST_COUNT)
        {
            return;
        }
        // usartPrintf("errorCheck:%d\t",errorCheck);
        // usartPrintf("flag:%d\t",flag);
        // usartPrintf("blinkTimes:%d\t",blinkTimes);
        // usartPrintf("Dt:%d\t", Dt);
        // usartPrintf("preDt:%d\n", preDt);
        // usartPrintf("colorCount:%d\n",fc_errors[errorCheck].colorCount);

        // usartPrintf("%d flag:%d dt:%d cnt:%d RGB:%d,%d,%d\n",errorCheck,flag,Dt,fc_errors[errorCheck].colorCount,fc_errors[errorCheck].color[0], fc_errors[errorCheck].color[1], fc_errors[errorCheck].color[2]);
        if (Dt > (preDt + FC_ERROR_BLINK_FAST_TIME))
        {
            // usartPrintf("FC_ERROR_BLINK_FAST_TIME\t");
            // usartPrintf("count:%d\n", fc_errors[errorCheck].colorCount);
            preDt = Dt;
            if (fc_errors[errorCheck].colorCount == 1)
            {

                if (!flag)
                {
                    // uint8_t r = fc_errors[errorCheck].color[0];
                    // uint8_t g = fc_errors[errorCheck].color[1];
                    // uint8_t b = fc_errors[errorCheck].color[2];
                    // usartPrintf("**r:%d\t", r);
                    // usartPrintf("g:%d\t", g);
                    // usartPrintf("b:%d\n", b);
                    rgb_set_color_sum(5, fc_errors[errorCheck].color[0], fc_errors[errorCheck].color[1], fc_errors[errorCheck].color[2]);
                }
                else
                {
                    // usartPrintf("rgb off\n", flag);
                    rgb_set_color_sum(5, 0, 0, 0);
                    blinkTimes++;
                }
                flag = !flag;
                // usartPrintf("rgb off\n", flag);
            }
            else if (fc_errors[errorCheck].colorCount == 2)
            {
                static uint8_t fcnt;
                if (fcnt == 0)
                {
                    // uint8_t r = fc_errors[errorCheck].color[0];
                    // uint8_t g = fc_errors[errorCheck].color[1];
                    // uint8_t b = fc_errors[errorCheck].color[2];
                    // usartPrintf("**r:%d\t", r);
                    // usartPrintf("g:%d\t", g);
                    // usartPrintf("b:%d\n", b);
                    // rgb_set_color(0,fc_errors[errorCheck].color[(fcnt*3)+0], fc_errors[errorCheck].color[(fcnt*3)+1], fc_errors[errorCheck].color[(fcnt*3)+2]);
                    rgb_set_color_sum(5, fc_errors[errorCheck].color[0], fc_errors[errorCheck].color[1], fc_errors[errorCheck].color[2]);
                }
                else if (fcnt == 1)
                {
                    // usartPrintf("rgb off\n", flag);
                    // rgb_set_color(0,0, 0, 0);
                    // fcnt++;
                    // blinkTimes++;
                    // rgb_set_color(0,0, 0, 0);
                    rgb_set_color_sum(5, fc_errors[errorCheck].color[3], fc_errors[errorCheck].color[4], fc_errors[errorCheck].color[5]);
                }
                // else if (fcnt == 2)
                // {
                //     // usartPrintf("rgb off\n", flag);

                //     // fcnt++;

                //     // rgb_set_color(0,fc_errors[errorCheck].color[3], fc_errors[errorCheck].color[4], fc_errors[errorCheck].color[5]);
                //     rgb_set_color(0,fc_errors[errorCheck].color[3], fc_errors[errorCheck].color[4], fc_errors[errorCheck].color[5]);

                // }
                // else if(fcnt == 3)
                // {
                //     rgb_set_color(0,0, 0, 0);
                // }

                fcnt++;

                if (fcnt > 1)
                {
                    fcnt = 0;
                    blinkTimes++;
                }
                if (blinkTimes == FC_ERROR_BLINK_FAST_COUNT)
                {
                    rgb_set_color_sum(5, 0, 0, 0);
                }

                // flag = !flag;

                // if(fcnt == (fc_errors[errorCheck].colorCount-1))
                // {
                //     fcnt=0;
                //     blinkTimes++;
                // }
            }
        }
    }
    else if (fc_errors[errorCheck].blinkType == FC_ERROR_BLINK_BREATH)
    {
        uint8_t r, g, b;
        // usartPrintf("fc_errors[errorCheck]:%d, %d, %d\n",fc_errors[errorCheck].color,fc_errors[errorCheck].blinkType,fc_errors[errorCheck].colorCount);
        if (fc_errors[errorCheck].colorCount == 1)
        {
            if (Dt < 500)
            {
                r = fc_errors[errorCheck].color[0] * Dt / 500;
                g = fc_errors[errorCheck].color[1] * Dt / 500;
                b = fc_errors[errorCheck].color[2] * Dt / 500;
            }
            else
            {
                if (Dt > 1000)
                    Dt = 1000;
                r = fc_errors[errorCheck].color[0] * (1000 - Dt) / 500;
                g = fc_errors[errorCheck].color[1] * (1000 - Dt) / 500;
                b = fc_errors[errorCheck].color[2] * (1000 - Dt) / 500;
            }
        }
        rgb_set_color_sum(5, r, g, b);
    }
    rgb_show();
}



void fc_error_handle_task(void)
{

    // FC_ERROR_SET(FC_IMU_CALIBRATING);

    while (1)
    {

        fc_error_rgb_handle();
        // fc_error_info_mesg();
        rt_thread_mdelay(5);
    }
}

void fc_error_handle_task_entry(void)
{

    rt_thread_t fc_error_handle_thread = rt_thread_create("fc_error_handle", fc_error_handle_task, RT_NULL, 512, 10, 10);
    if (fc_error_handle_thread != RT_NULL)
    {
        rt_thread_startup(fc_error_handle_thread);
    }
    else
    {
        LOG_E("fc_error_handle_thread create failed.");
    }
}
// ZINO_APP_EXPORT(fc_error_handle_task_entry);
